#include "RWSimPlugin.hpp"

#ifdef __WIN32
#include <windows.h>
#endif

#include <iostream>

#include <boost/foreach.hpp>
#include <boost/lexical_cast.hpp>
#include <xercesc/util/NumberFormatException.hpp>
#include <xercesc/util/XMLString.hpp>

#include <RobWorkStudio.hpp>

#include <rw/math/RPY.hpp>
#include <rw/math/Math.hpp>
#include <rw/math/Constants.hpp>
#include <rw/math/Transform3D.hpp>
#include <rw/kinematics/State.hpp>
#include <rw/kinematics/Kinematics.hpp>
#include <rw/models/JointDevice.hpp>

#include <rw/common/TimerUtil.hpp>
#include <rw/proximity/CollisionDetector.hpp>
#include <rw/proximity/Proximity.hpp>
#include <rw/loaders/path/PathLoader.hpp>

#include <rwsimlibs/gui/JointControlDialog.hpp>
#include <rwsimlibs/gui/SimCfgDialog.hpp>
#include <rwsimlibs/gui/CreateEngineDialog.hpp>
#include <rwsimlibs/gui/BodyControllerWidget.hpp>

#include <rwsim/dynamics/RigidBody.hpp>
#include <rwsim/loaders/DynamicWorkCellLoader.hpp>
#include <rwsim/drawable/SimulatorDebugRender.hpp>

#include <rwsim/dynamics/KinematicDevice.hpp>
#include <rwsim/dynamics/RigidDevice.hpp>

#include <rw/sensor/TactileArray.hpp>
#include <rwlibs/opengl/TactileArrayRender.hpp>
#include <rwlibs/simulation/SimulatedController.hpp>
#include <rwlibs/simulation/SimulatedSensor.hpp>
#include <rwlibs/control/JointController.hpp>

#include <rw/graspplanning/GraspTable.hpp>
#include <rwsim/control/BodyController.hpp>
#include <rwsimlibs/ode/ODESimulator.hpp>
#include "rwsim/control/BeamJointController.hpp"
//#include <rwsim/dynamics/SuctionCup.hpp>
//#include <rwsim/control/SuctionCupController1.hpp>

#include <rwlibs/task/Task.hpp>
#include <rwsim/simulator/GraspTask.hpp>
//#include <rwsim/simulator/GraspTaskSimulator.hpp>
#include <rwsim/util/SurfacePoseSampler.hpp>

#include "OwnQHullND.hpp"

using namespace boost::numeric::ublas;
using namespace rw::graspplanning;
using namespace rw::loaders;
using namespace rw::models;
using namespace rw::trajectory;
using namespace rw::math;
using namespace rw::kinematics;
using namespace rw::common;
using namespace rw::proximity;
using namespace rw::sensor;
using namespace rw::graphics;
using namespace rwlibs::opengl;
using namespace rwlibs::simulation;
using namespace rwlibs::control;
using namespace rw::geometry;

using namespace rwsim::dynamics;
using namespace rwsim::drawable;
using namespace rwsim::loaders;
using namespace rwsim::sensor;
using namespace rwsim::simulator;
using namespace rwsim::util;
using namespace rws;
using rwsim::control::BeamJointController;

using namespace rwlibs::task;
//using rwsim::control::SuctionCupController;
#define RW_DEBUGS( str ) //std::cout << str  << std::endl;

RWSimPlugin::RWSimPlugin():
																																																																																			RobWorkStudioPlugin("RWSimPlugin", QIcon(":/rwsimplugin/SimulationIcon.png")),
																																																																																			_dwc(NULL),
																																																																																			_sim(NULL)
{
	setupUi(this);

	connect(_openDwcBtn    ,SIGNAL(pressed()), this, SLOT(btnPressed()) );
	connect(_generateTasksBtn    ,SIGNAL(pressed()), this, SLOT(btnPressed()) );
	connect(_saveTasksBtn    ,SIGNAL(pressed()), this, SLOT(btnPressed()) );

	connect(_loadTasksBtn    ,SIGNAL(pressed()), this, SLOT(btnPressed()) );
	connect(_simulateBtn     ,SIGNAL(pressed()), this, SLOT(btnPressed()) );
	connect(_saveResultsBtn,SIGNAL(pressed()), this, SLOT(btnPressed()) );

	connect(_tasksSpinBox ,SIGNAL(valueChanged(int)), this, SLOT(changedEvent()) );

	_AccState = 0;

	_seed = clock();
	//_seed = 0;
	Math::seed( _seed );

	_timer = new QTimer( this );
	_timer->setInterval( 100 );
	connect( _timer, SIGNAL(timeout()), this, SLOT(btnPressed()) );
}


void RWSimPlugin::setupMenu(QMenu* menu){
	QMenu *dynMenu = getRobWorkStudio()->menuBar()->addMenu(tr("Dynamics"));

	_openAction = new QAction(QIcon(":/images/open.png"), tr("&Open DynamicWorkCell..."), this); // owned
	connect(_openAction, SIGNAL(triggered()), this, SLOT(btnPressed()));
	dynMenu->addAction(_openAction);
	dynMenu->addSeparator();
}

void RWSimPlugin::openDWC() {
	Ptr<DynamicWorkCell> dwc(NULL);
	try {
		dwc = DynamicWorkCellLoader::load(DWC_FILE);
	} catch (const Exception& exp) {
		QMessageBox::information(
				NULL,
				"Exception",
				exp.getMessage().getText().c_str(),
				QMessageBox::Ok);
		return;
	}
	_dwc = dwc;
	getRobWorkStudio()->setWorkcell( _dwc->getWorkcell() );
	if(_dwc==NULL) return;
	_dwc->setGravity(Vector3D<>::zero());
	_graspSim = ownedPtr(new OwnGraspTaskSimulator(_dwc));
}

void RWSimPlugin::btnPressed(){
	QObject *obj = sender();
	if( obj == _openDwcBtn || obj==_openAction ) {
		openDWC();
		std::vector<VectorND<6> > vertices;
		/*for (double i1 = -0.5; i1 <= 0.5; i1 = i1+1.0) {
			for (double i2 = -0.6; i2 <= 0.6; i2 = i2+1.2) {
				for (double i3 = -0.5; i3 <= 0.5; i3 = i3+1.0) {
					for (double i4 = -0.5; i4 <= 0.5; i4 = i4+1.0) {
						for (double i5 = -0.3; i5 <= 0.3; i5 = i5+0.6) {
							for (double i6 = -0.5; i6 <= 0.5; i6 = i6+1.0) {
								VectorND<6> vert;
								vert[0] = i1;
								vert[1] = i2;
								vert[2] = i3;
								vert[3] = i4;
								vert[4] = i5;
								vert[5] = i6;
								vertices.push_back(vert);
							}
						}
					}
				}
			}
		}*/
		VectorND<6> vert;
		vert[0] = -0.0204561;
		vert[1] = -0.404597;
		vert[2] = -0.265658;
		vert[3] = -0.0568215;
		vert[4] = -0.491515;
		vert[5] = -0.0242236;
		vertices.push_back(vert);
		vert[0] = -0.075114;
		vert[1] = -0.463077;
		vert[2] = -0.386064;
		vert[3] = -0.229863;
		vert[4] = -0.218424;
		vert[5] = 0.316745;
		vertices.push_back(vert);
		vert[0] = -0.309178;
		vert[1] = -0.0792879;
		vert[2] = -0.09769;
		vert[3] = -0.402696;
		vert[4] = 0.478381;
		vert[5] = -0.334532;
		vertices.push_back(vert);
		vert[0] = -0.277786;
		vert[1] = -0.148196;
		vert[2] = -0.171438;
		vert[3] = -0.295351;
		vert[4] = 0.0924036;
		vert[5] = 0.168428;
		vertices.push_back(vert);
		vert[0] = -0.150982;
		vert[1] = -0.390755;
		vert[2] = -0.326745;
		vert[3] = 0.111607;
		vert[4] = -0.159467;
		vert[5] = -0.366531;
		vertices.push_back(vert);
		vert[0] = -0.251631;
		vert[1] = -0.14853;
		vert[2] = -0.273403;
		vert[3] = 0.136669;
		vert[4] = -0.0332069;
		vert[5] = 0.331785;
		vertices.push_back(vert);
		vert[0] = -0.464225;
		vert[1] = -0.0573807;
		vert[2] = -0.435517;
		vert[3] = 0.429089;
		vert[4] = 0.485843;
		vert[5] = -0.222849;
		vertices.push_back(vert);
		vert[0] = -0.157731;
		vert[1] = -0.280892;
		vert[2] = -0.314405;
		vert[3] = 0.427498;
		vert[4] = 0.0653276;
		vert[5] = 0.112464;
		vertices.push_back(vert);
		vert[0] = -0.163423;
		vert[1] = -0.0367427;
		vert[2] = 0.238582;
		vert[3] = -0.000857177;
		vert[4] = -0.00395599;
		vert[5] = -0.170211;
		vertices.push_back(vert);
		vert[0] = -0.486951;
		vert[1] = -0.352327;
		vert[2] = 0.230553;
		vert[3] = -0.0335427;
		vert[4] = -0.453744;
		vert[5] = 0.0205421;
		vertices.push_back(vert);
		vert[0] = -0.266328;
		vert[1] = -0.476162;
		vert[2] = 0.0182274;
		vert[3] = -0.0914534;
		vert[4] = 0.280485;
		vert[5] = -0.253313;
		vertices.push_back(vert);
		vert[0] = -0.360496;
		vert[1] = -0.310561;
		vert[2] = 0.46608;
		vert[3] = -0.325815;
		vert[4] = 0.295169;
		vert[5] = 0.198097;
		vertices.push_back(vert);
		vert[0] = -0.302657;
		vert[1] = -0.289073;
		vert[2] = 0.0565861;
		vert[3] = 0.19724;
		vert[4] = -0.298701;
		vert[5] = -0.118861;
		vertices.push_back(vert);
		vert[0] = -0.0696349;
		vert[1] = -0.344913;
		vert[2] = 0.0681475;
		vert[3] = 0.0853187;
		vert[4] = -0.115891;
		vert[5] = 0.32437;
		vertices.push_back(vert);
		vert[0] = -0.301819;
		vert[1] = -0.109157;
		vert[2] = 0.0976632;
		vert[3] = 0.232917;
		vert[4] = 0.0213342;
		vert[5] = -0.355844;
		vertices.push_back(vert);
		vert[0] = -0.237167;
		vert[1] = -0.422356;
		vert[2] = 0.0552544;
		vert[3] = 0.0300284;
		vert[4] = 0.441998;
		vert[5] = 0.224259;
		vertices.push_back(vert);
		vert[0] = -0.252598;
		vert[1] = 0.240956;
		vert[2] = -0.385412;
		vert[3] = -0.027019;
		vert[4] = -0.453897;
		vert[5] = -0.122094;
		vertices.push_back(vert);
		vert[0] = -0.315777;
		vert[1] = 0.182106;
		vert[2] = -0.38575;
		vert[3] = -0.0367757;
		vert[4] = -0.199887;
		vert[5] = 0.357736;
		vertices.push_back(vert);
		vert[0] = -0.0839306;
		vert[1] = 0.427619;
		vert[2] = -0.102224;
		vert[3] = -0.124819;
		vert[4] = 0.0625964;
		vert[5] = -0.0717754;
		vertices.push_back(vert);
		vert[0] = -0.365057;
		vert[1] = 0.202463;
		vert[2] = -0.00734194;
		vert[3] = -0.0417471;
		vert[4] = 0.423058;
		vert[5] = 0.478204;
		vertices.push_back(vert);
		vert[0] = -0.254744;
		vert[1] = 0.300791;
		vert[2] = -0.037646;
		vert[3] = 0.451185;
		vert[4] = -0.300847;
		vert[5] = -0.178697;
		vertices.push_back(vert);
		vert[0] = -0.221869;
		vert[1] = 0.269079;
		vert[2] = -0.415097;
		vert[3] = 0.141921;
		vert[4] = -0.0219818;
		vert[5] = 0.411343;
		vertices.push_back(vert);
		vert[0] = -0.331166;
		vert[1] = 0.214302;
		vert[2] = -0.419758;
		vert[3] = 0.286524;
		vert[4] = 0.26857;
		vert[5] = -0.142527;
		vertices.push_back(vert);
		vert[0] = -0.0547014;
		vert[1] = 0.084061;
		vert[2] = -0.261228;
		vert[3] = 0.10078;
		vert[4] = 0.131643;
		vert[5] = 0.105857;
		vertices.push_back(vert);
		vert[0] = -0.00648401;
		vert[1] = 0.299988;
		vert[2] = 0.0939971;
		vert[3] = -0.154672;
		vert[4] = -0.205637;
		vert[5] = -0.121291;
		vertices.push_back(vert);
		vert[0] = -0.372129;
		vert[1] = 0.385594;
		vert[2] = 0.29054;
		vert[3] = -0.47937;
		vert[4] = -0.350147;
		vert[5] = 0.474251;
		vertices.push_back(vert);
		vert[0] = -0.459374;
		vert[1] = 0.265068;
		vert[2] = 0.430389;
		vert[3] = -0.187727;
		vert[4] = 0.190804;
		vert[5] = -0.122541;
		vertices.push_back(vert);
		vert[0] = -0.375687;
		vert[1] = 0.103666;
		vert[2] = 0.429576;
		vert[3] = -0.0217615;
		vert[4] = 0.244044;
		vert[5] = 0.497809;
		vertices.push_back(vert);
		vert[0] = -0.423092;
		vert[1] = 0.221773;
		vert[2] = 0.150047;
		vert[3] = 0.343137;
		vert[4] = -0.217455;
		vert[5] = -0.100482;
		vertices.push_back(vert);
		vert[0] = -0.277918;
		vert[1] = 0.457543;
		vert[2] = 0.426914;
		vert[3] = 0.121112;
		vert[4] = -0.427772;
		vert[5] = 0.483292;
		vertices.push_back(vert);
		vert[0] = -0.46754;
		vert[1] = 0.356044;
		vert[2] = 0.497383;
		vert[3] = 0.295565;
		vert[4] = 0.276737;
		vert[5] = -0.233503;
		vertices.push_back(vert);
		vert[0] = -0.121696;
		vert[1] = 0.191899;
		vert[2] = 0.347161;
		vert[3] = 0.211741;
		vert[4] = 0.377652;
		vert[5] = 0.19409;
		vertices.push_back(vert);
		vert[0] = 0.424069;
		vert[1] = -0.489968;
		vert[2] = -0.227605;
		vert[3] = -0.350953;
		vert[4] = -0.206614;
		vert[5] = -0.389486;
		vertices.push_back(vert);
		vert[0] = 0.449686;
		vert[1] = -0.39341;
		vert[2] = -0.2797;
		vert[3] = -0.268374;
		vert[4] = -0.0219148;
		vert[5] = 0.410118;
		vertices.push_back(vert);
		vert[0] = 0.31216;
		vert[1] = -0.41233;
		vert[2] = -0.0245318;
		vert[3] = -0.114553;
		vert[4] = 0.0354232;
		vert[5] = -0.178828;
		vertices.push_back(vert);
		vert[0] = 0.402836;
		vert[1] = -0.422654;
		vert[2] = -0.188262;
		vert[3] = -0.467087;
		vert[4] = 0.0251841;
		vert[5] = 0.228564;
		vertices.push_back(vert);
		vert[0] = 0.264194;
		vert[1] = -0.477119;
		vert[2] = -0.297579;
		vert[3] = 0.377611;
		vert[4] = -0.0575792;
		vert[5] = -0.0876329;
		vertices.push_back(vert);
		vert[0] = 0.347893;
		vert[1] = -0.484202;
		vert[2] = -0.277879;
		vert[3] = 0.319259;
		vert[4] = -0.325978;
		vert[5] = 0.0740843;
		vertices.push_back(vert);
		vert[0] = 0.465719;
		vert[1] = -0.406928;
		vert[2] = -0.301446;
		vert[3] = 0.459532;
		vert[4] = 0.430296;
		vert[5] = -0.228101;
		vertices.push_back(vert);
		vert[0] = 0.39861;
		vert[1] = -0.0368778;
		vert[2] = -0.242034;
		vert[3] = 0.261014;
		vert[4] = 0.373426;
		vert[5] = 0.308314;
		vertices.push_back(vert);
		vert[0] = 0.47784;
		vert[1] = -0.283895;
		vert[2] = 0.0758469;
		vert[3] = -0.430702;
		vert[4] = -0.420261;
		vert[5] = -0.196262;
		vertices.push_back(vert);
		vert[0] = 0.227954;
		vert[1] = -0.4465;
		vert[2] = 0.142382;
		vert[3] = -0.377003;
		vert[4] = -0.401976;
		vert[5] = 0.372416;
		vertices.push_back(vert);
		vert[0] = 0.176663;
		vert[1] = -0.470075;
		vert[2] = 0.10053;
		vert[3] = -0.412884;
		vert[4] = 0.246366;
		vert[5] = -0.241975;
		vertices.push_back(vert);
		vert[0] = 0.20192;
		vert[1] = -0.376007;
		vert[2] = 0.00433292;
		vert[3] = -0.480961;
		vert[4] = 0.328493;
		vert[5] = 0.067693;
		vertices.push_back(vert);
		vert[0] = 0.0264929;
		vert[1] = -0.197066;
		vert[2] = 0.252646;
		vert[3] = 0.136991;
		vert[4] = -0.106232;
		vert[5] = -0.000803685;
		vertices.push_back(vert);
		vert[0] = 0.0246923;
		vert[1] = -0.19049;
		vert[2] = 0.46385;
		vert[3] = 0.1238;
		vert[4] = -0.122716;
		vert[5] = 0.318074;
		vertices.push_back(vert);
		vert[0] = 0.287188;
		vert[1] = -0.153725;
		vert[2] = 0.0221863;
		vert[3] = 0.40519;
		vert[4] = 0.0408211;
		vert[5] = -0.411751;
		vertices.push_back(vert);
		vert[0] = 0.320267;
		vert[1] = -0.0291831;
		vert[2] = 0.0364882;
		vert[3] = 0.43079;
		vert[4] = 0.491773;
		vert[5] = 0.46149;
		vertices.push_back(vert);
		vert[0] = 0.00999531;
		vert[1] = 0.233724;
		vert[2] = -0.239127;
		vert[3] = -0.324499;
		vert[4] = -0.403763;
		vert[5] = -0.232921;
		vertices.push_back(vert);
		vert[0] = 0.214435;
		vert[1] = 0.134009;
		vert[2] = -0.439913;
		vert[3] = -0.10912;
		vert[4] = -0.0917183;
		vert[5] = 0.315935;
		vertices.push_back(vert);
		vert[0] = 0.152725;
		vert[1] = 0.455395;
		vert[2] = -0.069532;
		vert[3] = -0.410745;
		vert[4] = 0.111904;
		vert[5] = -0.0436443;
		vertices.push_back(vert);
		vert[0] = 0.249265;
		vert[1] = 0.381562;
		vert[2] = -0.0754161;
		vert[3] = -0.112854;
		vert[4] = 0.257492;
		vert[5] = 0.420072;
		vertices.push_back(vert);
		vert[0] = 0.0654208;
		vert[1] = 0.37913;
		vert[2] = -0.018365;
		vert[3] = 0.0955724;
		vert[4] = -0.161658;
		vert[5] = -0.146209;
		vertices.push_back(vert);
		vert[0] = 0.30393;
		vert[1] = 0.461563;
		vert[2] = -0.221744;
		vert[3] = 0.037089;
		vert[4] = -0.212212;
		vert[5] = 0.145629;
		vertices.push_back(vert);
		vert[0] = 0.069019;
		vert[1] = 0.081694;
		vert[2] = -0.14268;
		vert[3] = 0.234883;
		vert[4] = 0.457115;
		vert[5] = -0.0380497;
		vertices.push_back(vert);
		vert[0] = 0.393415;
		vert[1] = 0.353321;
		vert[2] = -0.381699;
		vert[3] = 0.425195;
		vert[4] = 0.135923;
		vert[5] = 0.43325;
		vertices.push_back(vert);
		vert[0] = 0.316278;
		vert[1] = 0.0460655;
		vert[2] = 0.117558;
		vert[3] = -0.337677;
		vert[4] = -0.15462;
		vert[5] = -0.399856;
		vertices.push_back(vert);
		vert[0] = 0.313628;
		vert[1] = 0.376114;
		vert[2] = 0.432876;
		vert[3] = -0.362767;
		vert[4] = -0.101415;
		vert[5] = 0.230485;
		vertices.push_back(vert);
		vert[0] = 0.363857;
		vert[1] = 0.281073;
		vert[2] = 0.458735;
		vert[3] = -0.495602;
		vert[4] = 0.406742;
		vert[5] = -0.243023;
		vertices.push_back(vert);
		vert[0] = 0.0653205;
		vert[1] = 0.142281;
		vert[2] = 0.0250435;
		vert[3] = -0.317828;
		vert[4] = 0.429397;
		vert[5] = 0.209031;
		vertices.push_back(vert);
		vert[0] = 0.208766;
		vert[1] = 0.271762;
		vert[2] = 0.311839;
		vert[3] = 0.371354;
		vert[4] = -0.0541455;
		vert[5] = -0.371906;
		vertices.push_back(vert);
		vert[0] = 0.498212;
		vert[1] = 0.49524;
		vert[2] = 0.12127;
		vert[3] = 0.00913922;
		vert[4] = -0.396796;
		vert[5] = 0.264754;
		vertices.push_back(vert);
		vert[0] = 0.257413;
		vert[1] = 0.228066;
		vert[2] = 0.438061;
		vert[3] = 0.269152;
		vert[4] = 0.350671;
		vert[5] = -0.485043;
		vertices.push_back(vert);
		vert[0] = 0.372741;
		vert[1] = 0.126872;
		vert[2] = 0.325627;
		vert[3] = 0.167215;
		vert[4] = 0.443343;
		vert[5] = 0.417841;
		vertices.push_back(vert);

		rw::common::Ptr<rw::geometry::OwnQHullND<6> > _chullCalculator = new OwnQHullND<6>();
		_chullCalculator->rebuild( vertices );
		VectorND<6> or6D = VectorND<6>::zero();
		if (_chullCalculator->getMinDistInside( or6D, vertices) > 0) {
			_chullCalculator->subspace( vertices, VectorND<6>::zero() );

			std::vector<VectorND<6> > vertList = _chullCalculator->getHullVertices();
			std::vector<VectorND<3> > vertListN2;
			for (size_t i = 0; i < vertList.size(); i++) {
				VectorND<3> vertN;
				vertN[0] = vertList[i][0];
				vertN[1] = vertList[i][1];
				vertN[2] = vertList[i][2];
				vertListN2.push_back(vertN);
			}
			rw::common::Ptr<rw::geometry::OwnQHullND<3> > hull2 = rw::common::ownedPtr(new rw::geometry::OwnQHullND<3>() );
			hull2->rebuild(vertListN2);

			VectorND<3> originN = VectorND<3>::zero();
			std::cout << hull2->getMinDistInside(originN) << std::endl;
		}
		char str[40] = "tmp/trimeshSphere.stl";
		const std::string file = str;
		rw::geometry::Sphere* sphere = new Sphere(1.03);
		rw::geometry::TriMesh::Ptr mesh = sphere->createMesh(100);
		rw::geometry::STLFile::save(*mesh,file);
		/*OwnQHullND<3> *qhull = new OwnQHullND<3>();
		std::vector<VectorND<3> > vertices;
		//vertices.push_back(VectorND<3>::zero());
		VectorND<3> vertex1, vertex2, vertex3, vertex4, vertex5, vertex6, vertex7, vertex8;*/
		/*vertex1[0] = 4.;
		vertex1[1] = -1.;
		vertex1[2] = 1.;
		vertex2[0] = 4.;
		vertex2[1] = -1.;
		vertex2[2] = -1.;
		vertex3[0] = 6.;
		vertex3[1] = 1.;
		vertex3[2] = 1.;
		vertex4[0] = 6.;
		vertex4[1] = 1.;
		vertex4[2] = -1.;
		vertex5[0] = -1.;
		vertex5[1] = 4.;
		vertex5[2] = 1.;
		vertex6[0] = -1.;
		vertex6[1] = 4.;
		vertex6[2] = -1.;
		vertex7[0] = 1.;
		vertex7[1] = 6.;
		vertex7[2] = 1.;
		vertex8[0] = 1.;
		vertex8[1] = 6.;
		vertex8[2] = -1.;*/
		/*vertex1[0] = -3.;
		vertex1[1] = 2.;
		vertex1[2] = 1.;
		vertex2[0] = -2.;/////////////
		vertex2[1] = 2.;
		vertex2[2] = -1.;
		vertex3[0] = -1.;
		vertex3[1] = 4.;
		vertex3[2] = 1.;
		vertex4[0] = -1.;
		vertex4[1] = 3.;//////////////
		vertex4[2] = -1.;
		vertex5[0] = 4.;
		vertex5[1] = -1.;
		vertex5[2] = 1.;
		vertex6[0] = 3.;////////////
		vertex6[1] = -1.;
		vertex6[2] = -1.;
		vertex7[0] = 2.;
		vertex7[1] = -3.;
		vertex7[2] = 1.;
		vertex8[0] = 2.;
		vertex8[1] = -2.;
		vertex8[2] = -1.;*/
		/*vertices.push_back(vertex1);
		vertices.push_back(vertex2);
		vertices.push_back(vertex3);
		vertices.push_back(vertex4);
		vertices.push_back(vertex5);
		vertices.push_back(vertex6);
		vertices.push_back(vertex7);
		vertices.push_back(vertex8);
		qhull->subspace(vertices);
		std::cout << "--------Face Normals-----------" << std::endl;
		std::vector<VectorND<3> > normals = qhull->getFaceNormals();
		std::vector<double> offsets = qhull->getFaceOffsets();
		for (int i = 0; i < normals.size(); i++) {
			std::cout << normals.at(i) << " " << offsets.at(i) << std::endl;
		}
		std::cout << "--------Vertices-----------" << std::endl;
		std::vector<VectorND<3> > vertices2 = qhull->getHullVertices();
		for (int i = 0; i < vertices2.size(); i++) {
			std::cout << vertices2.at(i) << std::endl;
		}
		std::cout << "---------MinDistInside----------" << std::endl;
		VectorND<3> origin = VectorND<3>::zero();*/
		/*origin[0] = 2.5;
		origin[1] = 2.5;
		origin[2] = 0.;*/
		//std::cout << qhull->getMinDistInside(origin, vertices) << std::endl;
		//std::cout << qhull->getMinDistOutside(origin, vertices) << std::endl;
	} else if( obj == _generateTasksBtn ) {
		int ntargets = _tasksSpinBox->value();
		// Assumption: 1 body available
		CartesianTask::Ptr tasks = new CartesianTask();
		SurfacePoseSampler ssurf(_dwc->getBodies()[0]->getGeometry());
		for (int i = 1; i <= ntargets; i++) {
			// Assumption: Body is placed in world origo
			Transform3D<> sample = ssurf.sample();
			CartesianTarget::Ptr target = new CartesianTarget(sample);
			tasks->addTarget(target);
		}
		tasks->getPropertyMap().set<std::string >("Gripper", "PG70");
		tasks->getPropertyMap().set<std::string >("TCP", "TCPPG70");
		//tasks->getPropertyMap().set<std::string >("Gripper", "SchunkHand");
		//tasks->getPropertyMap().set<std::string >("TCP", "SchunkHand.TCP");
		tasks->getPropertyMap().set<std::string >("Object", "Object");
		Q openQ(1, 0.034);
		Q closeQ(1, 0.0);
		//Q openQ(7, -0.6,0,1,-0.6,0,-0.6,0);
		//Q closeQ(7, 0,0.4,1,0,0.4,0,0.4);
		tasks->getPropertyMap().set<Q>("OpenQ", openQ);
		tasks->getPropertyMap().set<Q>("CloseQ", closeQ);
		/*Vector3D<> pos(0,0,-0.04);
		Transform3D<> Tretract(pos);
		tasks->getPropertyMap().set<Transform3D<> >("Retract", Tretract);*/
		_tasks = tasks;
	} else if( obj == _saveTasksBtn ) {
		GraspTask gtask(_tasks);
		GraspTask::saveRWTask(&gtask,"saved_tasks.task.xml");
	} else if( obj == _loadTasksBtn ) {
		CartesianTask::Ptr task;
		try {
			XMLTaskLoader loader;
			loader.load("saved_tasks.task.xml");
			task = loader.getCartesianTask();
		} catch (const Exception& exp) {
			return;
		}
		_tasks = task;
		_graspSim->load("saved_tasks.task.xml");
	} else if( obj == _simulateBtn && _AccState == 0) {
		_startState = getRobWorkStudio()->getState();
		statemachine();
	} else if( obj == _saveResultsBtn ) {
		FILE* from = fopen("tmp/result.txt", "rb");
		FILE* to = fopen("result.txt", "wb");
		if(from!=NULL) {
			if(to!=NULL) {
				while(!feof(from)) {
					char ch = fgetc(from);
					if(!ferror(from) && !ferror(to) && !feof(from)) fputc(ch, to);
				}
				fclose(from);
				fclose(to);
			}
		}
	} else if(obj==_timer){
		if (_graspSim->isFinished()) statemachine();

		if(_graspSim->getSimulator()==NULL)
			return;
		if (!_graspSim->isFinished() && _visualizeCheckBox->isChecked()) {
			_graspSim->pauseSimulation();
			State state = _graspSim->getSimulator()->getState();
			getRobWorkStudio()->setState(state);
			TimerUtil::sleepMs(200);
			_graspSim->resumeSimulation();
		}
	}
}

void RWSimPlugin::statemachine() {
	static std::vector<int>* goodTargets = new std::vector<int>();
	static size_t goodTargetsIndex;
	static CartesianTask::Ptr tempTask;
	static CartesianTarget::Ptr notSimulatedTarget;
	std::cout << "Statemachine " << _AccState << " " << _curTask << std::endl;
	double g = 9.82*2.5;
	double timeStep = 0.001;
	//Q openQ(7, -0.6,0,1,-0.6,0,-0.6,0);
	//Q closeQ(7, -0.6,0,1,-0.6,0,-0.6,0);
	Q openQ(1, 0.034);
	Q closeQ(1, 0.0);

	_timer->stop();

	if (_AccState == 0) {
		_AccState = 1;
		_curTask = 0;
		delete goodTargets;
		goodTargets = new std::vector<int>();
	} else {
		/*if (_curTask == 0) tempTask = new CartesianTask();
		Transform3D<> newTargetT = _graspSim->getResult()->getTargets()[0]->get();
		CartesianTarget::Ptr newTarget = new CartesianTarget(newTargetT);
		rw::common::PropertyMap newPM;
		newPM = _graspSim->getResult()->getTargets()[0]->getPropertyMap();
		newTarget->setPropertyMap(newPM);
		tempTask->addTarget(newTarget);*/
		CartesianTask::Ptr task = _graspSim->getResult();
		if (_curTask == 0) {
			task->removeTarget(task->getTargets()[1]);
			tempTask = task;
		}
		else tempTask->addTarget(task->getTargets()[0]);
		if (_AccState == 1) {
			if (tempTask->getTargets()[_curTask]->getPropertyMap().get<int>("TestStatus") == 1) {
				goodTargets->insert(goodTargets->end(),_curTask);
			} else {
				notSimulatedTarget = tempTask->getTargets()[_curTask];
			}
		} else if (_AccState > 1) {
			int nextTask;
			if (goodTargets->size()-1 >= goodTargetsIndex) {
				nextTask = goodTargets->at(goodTargetsIndex);
				goodTargetsIndex++;
			} else {
				nextTask = _tasks->getTargets().size();
				goodTargetsIndex = 0;
			}
			for (int i = _curTask; i < nextTask-1; i++) {
				tempTask->addTarget(notSimulatedTarget);
				_curTask++;
			}
		}
		_curTask++;
	}
	std::cout << "a" << std::endl;
	if ((size_t)_curTask == _tasks->getTargets().size()) {
		std::cout << "State " << _AccState << " finished." << std::endl;
		char fname[50];
		sprintf(fname, "tmp/test%i.xml",_AccState);
		saveResultXML(tempTask, fname,true);
		saveResults(tempTask, _AccState);
		_AccState++;
		_curTask = 0;
	}
	std::cout << "b" << std::endl;

	_graspSim->pauseSimulation();
	//std::cout << "ba" << std::endl;
	while (_graspSim->isRunning());
	std::cout << "bb" << std::endl;
	_state = _startState;
	//delete _graspSim.get();
	openDWC();
	std::string handName = _tasks->getPropertyMap().get<std::string>("Gripper");
	_dwc->findDevice(handName)->getKinematicModel()->setQ(openQ, _state);
	getRobWorkStudio()->setState(_state);
	std::cout << "c" << std::endl;

	CartesianTask::Ptr singleTask = ownedPtr(new CartesianTask());
	Transform3D<> targetT = _tasks->getTargets()[_curTask]->get();
	CartesianTarget::Ptr target = ownedPtr(new CartesianTarget(targetT));
	singleTask->addTarget(target);
	singleTask->addTarget(target);
	if (_AccState == 1) singleTask->getPropertyMap().set<Vector3D<> >("AccWGripperClosed", Vector3D<>::zero());
	else if (_AccState == 2) singleTask->getPropertyMap().set<Vector3D<> >("AccWGripperClosed", Vector3D<>::x()*1*g);
	else if (_AccState == 3) singleTask->getPropertyMap().set<Vector3D<> >("AccWGripperClosed", Vector3D<>::x()*-1*g);
	else if (_AccState == 4) singleTask->getPropertyMap().set<Vector3D<> >("AccWGripperClosed", Vector3D<>::y()*1*g);
	else if (_AccState == 5) singleTask->getPropertyMap().set<Vector3D<> >("AccWGripperClosed", Vector3D<>::y()*-1*g);
	else if (_AccState == 6) singleTask->getPropertyMap().set<Vector3D<> >("AccWGripperClosed", Vector3D<>::z()*1*g);
	else if (_AccState == 7) singleTask->getPropertyMap().set<Vector3D<> >("AccWGripperClosed", Vector3D<>::z()*-1*g);
	singleTask->getPropertyMap().set<std::string >("Gripper", "PG70");
	singleTask->getPropertyMap().set<std::string >("TCP", "TCPPG70");
	//singleTask->getPropertyMap().set<std::string >("Gripper", "SchunkHand");
	//singleTask->getPropertyMap().set<std::string >("TCP", "SchunkHand.TCP");
	singleTask->getPropertyMap().set<std::string >("Object", "Object");
	singleTask->getPropertyMap().set<Q>("OpenQ", openQ);
	singleTask->getPropertyMap().set<Q>("CloseQ", closeQ);
	GraspTask gtask(singleTask);
	GraspTask::saveRWTask(&gtask,"tmp/curTask.task.xml");

	if (_AccState < 8) {
		std::cout << "d" << std::endl;
		_graspSim->load("tmp/curTask.task.xml");
		_graspSim->init(_dwc, _state);
		_graspSim->getSimulator()->setTimeStep(timeStep);
		Math::seed( _seed );
		_graspSim->startSimulation(_state);
		while (!_graspSim->isRunning());
	}

	std::cout << "e" << std::endl;
	//if (_AccState == 1 && _curTask == 0) _timer->start();
	if (_AccState != 8) _timer->start();
	if (_AccState == 8) {
		//_timer->stop();
		_AccState = 0;
		_curTask = 0;
	}
}

CartesianTask::Ptr RWSimPlugin::saveResultXML(CartesianTask::Ptr &tasks, const std::string & name, bool removeFailures) {
	std::vector<int> returnIDs(0);
	CartesianTask::Ptr successResult = tasks->clone();
	successResult->getTargets().resize(0);
	if (removeFailures) {
		BOOST_FOREACH(CartesianTarget::Ptr target, tasks->getTargets()) {
			//int status = target->getPropertyMap().get<int>("TestStatus", -1);
			//if (status == OwnGraspTaskSimulator::Success || status == OwnGraspTaskSimulator::ObjectDropped || status == OwnGraspTaskSimulator::ObjectSlipped) {
			successResult->getTargets().push_back(target);
			returnIDs.push_back(target->getIndex());
			//}
		}
		_successIDs = returnIDs;
	} else {
		BOOST_FOREACH(CartesianTarget::Ptr target, tasks->getTargets()) {
			BOOST_FOREACH(int index, _successIDs) {
				int id = target->getIndex();
				if (id == index) {
					successResult->getTargets().push_back(target);
				}
			}
		}
	}
	CartesianTask::Ptr wrapTask = new CartesianTask();
	wrapTask->addTask(successResult);
	GraspTask gtask(wrapTask);
	GraspTask::saveRWTask(&gtask,name);
	return successResult;
}

void RWSimPlugin::saveResults(CartesianTask::Ptr &tasks, int state) {
	char line[256];
	FILE* file;
	if (state == 1) file = fopen("tmp/result.txt", "w+");
	else file = fopen("tmp/result.txt", "r");
	std::ostringstream buf;
	if (state == 1) {
		buf << std::setw(6) << "#" << ", ";
		buf << std::setw(9) << "Wrench" << ", " << std::setw(6) << "Grebet" << ", " << std::setw(4) << "Tabt" << std::endl;
	} else {
		std::string str;
		if (fgets(line, sizeof line, file) != NULL) str = line;
		str = str.substr(0,str.size()-1);
		buf << str;
		if (state == 2) buf << ", " << std::setw(9) << "Wrench" << ", " << std::setw(4) << "G_Xp" << ", " << std::setw(4) << "T_Xp";
		else if (state == 3) buf << ", " << std::setw(9) << "Wrench" << ", " << std::setw(4) << "G_Xn" << ", " << std::setw(4) << "T_Xn";
		else if (state == 4) buf << ", " << std::setw(9) << "Wrench" << ", " << std::setw(4) << "G_Yp" << ", " << std::setw(4) << "T_Yp";
		else if (state == 5) buf << ", " << std::setw(9) << "Wrench" << ", " << std::setw(4) << "G_Yn" << ", " << std::setw(4) << "T_Yn";
		else if (state == 6) buf << ", " << std::setw(9) << "Wrench" << ", " << std::setw(4) << "G_Zp" << ", " << std::setw(4) << "T_Zp";
		else if (state == 7) buf << ", " << std::setw(9) << "Wrench" << ", " << std::setw(4) << "G_Zn" << ", " << std::setw(4) << "T_Zn";
		buf << std::endl;
	}
	BOOST_FOREACH(CartesianTarget::Ptr target, tasks->getTargets()) {
		std::string str;
		if (fgets(line, sizeof line, file) != NULL) str = line;
		str = str.substr(0,str.size()-1);
		buf << str;
		int status = target->getPropertyMap().get<int>("TestStatus", -1);
		int gripped;
		int dropped;
		if (status == OwnGraspTaskSimulator::Success || status == OwnGraspTaskSimulator::ObjectDropped || status == OwnGraspTaskSimulator::ObjectSlipped ) {
			gripped = 1;
			if (status == OwnGraspTaskSimulator::Success) dropped = 0;
			else dropped = 1;
		} else {
			if (status == OwnGraspTaskSimulator::TimeOut ||
					status == OwnGraspTaskSimulator::SimulationFailure ||
					status == OwnGraspTaskSimulator::InvKinFailure ||
					status == OwnGraspTaskSimulator::PoseEstimateFailure) {
				gripped = -1;
			} else gripped = 0;
			dropped = -1;
		}
		int width = 4;
		if (state == 1) width = 6;
		Q wrench = target->getPropertyMap().get<Q>("QualityBeforeLifting", Q::zero(5));
		if (state == 1) buf << std::setw(6) << target->getIndex() << ", ";
		if (state != 1) buf << ", ";
		buf << std::setprecision(6) << std::fixed << std::setw(9) << wrench[0];
		buf << ", ";
		if (gripped  < 0) buf << std::setw(width) << "-1"; // Some strange error
		if (gripped == 0) buf << std::setw(width) << "0";
		if (gripped  > 0) buf << std::setw(width) << "1";
		buf << ", ";
		if (dropped  < 0) buf << std::setw(4) << "-1"; // Does not give any sense
		if (dropped == 0) buf << std::setw(4) << "1";
		if (dropped  > 0) buf << std::setw(4) << "0";
		buf << std::endl;
	}
	/*if (state == 1) {
		buf << std::setw(6) << "#" << ", " << std::setw(6) << "ContPt" << ", ";
		buf << std::setw(9) << "x" << ", " << std::setw(9) << "y" << ", " << std::setw(9) << "z" << std::endl;
		BOOST_FOREACH(CartesianTarget::Ptr target, tasks->getTargets()) {
			int points = target->getPropertyMap().get<int>("ContactPoints",0);
			for (int i = 0; i < points; i++) {
				buf << std::setw(6) << target->getIndex() << ", " << std::setw(6) << i << ", ";
				Vector3D<> point = target->getPropertyMap().get<Vector3D<> >("ContactPointPos"+boost::lexical_cast<std::string>(i), Vector3D<>::zero());
				buf << std::setprecision(6) << std::fixed << std::setw(9) << point[0];
				buf << ", ";
				buf << std::setprecision(6) << std::fixed << std::setw(9) << point[1];
				buf << ", ";
				buf << std::setprecision(6) << std::fixed << std::setw(9) << point[2];
				buf << std::endl;
			}
		}
	}*/
	std::string str;
	while (fgets(line, sizeof line, file) != NULL) {
		str = line;
		str = str.substr(0,str.size()-1);
		buf << str;
		buf << std::endl;
	}
	fclose(file);
	std::ofstream outfile;
	outfile.open("tmp/result.txt");
	outfile << buf.str();
	outfile.close();
}

void RWSimPlugin::stepCallBack(const rw::kinematics::State& state){
	_state = state;
	//TimerUtil::sleepMs(500);
	getRobWorkStudio()->postState(_state);
}

void RWSimPlugin::changedEvent(){
	//QObject *obj = sender();
	/*if( obj == _x ){
    }*/
}

void RWSimPlugin::updateStatus(){
}

void RWSimPlugin::open(rw::models::WorkCell* workcell){
	if( workcell==NULL || _dwc==NULL ){
		return;
	}

	_state = getRobWorkStudio()->getState();

	// add sensor drawables to the workcell drawer
	BOOST_FOREACH(SimulatedSensor::Ptr sensor,  _dwc->getSensors()){
		if( dynamic_cast<TactileArray*>(sensor.get()) ){
			//std::cout << "ADDING TACTILE SENSOR DRAWER..." << std::endl;
			TactileArray *tsensor = dynamic_cast<TactileArray*>(sensor.get());
			TactileArrayRender *render = new TactileArrayRender(tsensor);
			Drawable *drawable = new Drawable(ownedPtr<Render>(render), sensor->getSensor()->getName() );
			//getRobWorkStudio()->getWorkCellGLDrawer()->addDrawableToFrame(workcell->getWorldFrame(), drawable);
			//std::cout << "TO: " << sensor->getFrame()->getName() << std::endl;

			getRobWorkStudio()->getWorkCellScene()->addDrawable(drawable, tsensor->getFrame() );
		}
	}

	BOOST_FOREACH(DynamicDevice* device, _dwc->getDynamicDevices()){
		rw::models::Device *dev = &device->getModel();
		if( dynamic_cast<JointDevice*>(dev) == NULL )
			continue;
		RW_ASSERT(dev);
		//std::cout << "Dev name: " << std::endl;
		//std::cout << dev->getName() << std::endl;
	}

	BOOST_FOREACH(SimulatedController::Ptr ctrl, _dwc->getControllers()){
		RW_ASSERT(ctrl!=NULL);
	}
}

void RWSimPlugin::close(){

}

void RWSimPlugin::initialize(){
	getRobWorkStudio()->stateChangedEvent().add(
			boost::bind(&RWSimPlugin::stateChangedListener, this, _1), this);
}

void RWSimPlugin::stateChangedListener(const State& state){
	_state = state;
	updateDialog();
}

Q_EXPORT_PLUGIN(RWSimPlugin);
